#include "HMM.h"
#include <math.h>
#include "HMM_Helper.h"

#define PI 4.0*atan(1.0)


HMM_State::HMM_State()
{
	_gconst = 0;
}

HMM_State::~HMM_State()
{
}


void HMM_State::SetMean(vector<double> mean)
{
	_mean = mean;
}
void HMM_State::SetVariance(vector<double> variance)
{
	_variance = variance;
}

void HMM_State::SetGConst(double gconst)
{
	_gconst = gconst;
}

bool HMM_State::CheckGConst()
{
	double sum = 0;
	for (unsigned int i=0; i<_variance.size(); i++)
		sum = 2*PI*_variance[i]*_variance[i];
	sum = log(sum);

	return sum-_gconst < 1e-03;
	
}

double HMM_State::GetObservationLikelihoodLog(vector<double> mfcc)
{
	// returns the logarithmic observation likelihood given a 39 dimensional feature vector
	
	/*double b = 0.0;

	for (unsigned int i=0; i<_mean.size(); i++)
	{
		b += log(2*PI) + pow(_variance[i],2) + pow(mfcc[i]-_mean[i],2)/pow(_variance[i],2);
	}

	return exp(-0.5*b);*/

	double b = 1.0;

	for (unsigned int i=0; i<_mean.size(); i++)
	{
		b *= 1/sqrt(2*PI*_variance[i]) * exp(-0.5* pow(mfcc[i]-_mean[i],2)/_variance[i]);
	}

	return log(b);

}

istream& operator >>(istream &is,HMM_State &obj)
{
	string line;
	vector<string> tokens;

	// e.g mean 39
	getline (is,line);
	tokens = HMM_Helper::Split(line);
	int itemCnt = atoi(tokens[1].c_str());
	// mean values
	getline(is, line);
	obj._mean = HMM_Helper::ConvertToDoubleVector(line);

	if (obj._mean.size() != itemCnt)
		cout << "Error in hmm definitions!" << endl;

	// e.g var 39
	getline (is,line);
	tokens = HMM_Helper::Split(line);
	itemCnt = atoi(tokens[1].c_str());
	// var values
	getline(is, line);
	obj._variance = HMM_Helper::ConvertToDoubleVector(line);

	if (obj._variance.size() != itemCnt)
		cout << "Error in hmm definitions!" << endl;

	// gconst
	getline (is,line);
	tokens = HMM_Helper::Split(line);
	obj._gconst = atof(tokens[1].c_str());

	cout << obj.CheckGConst() << " ";

	return is;
}


HMM::HMM()
{
	_label = "";
	_numstates = 0;
}

HMM::HMM(std::string label)
{
	_label = label;
	_numstates = 0;
}


HMM::~HMM(void)
{
	for (int i=0; i<_numstates; i++)
		delete _states.at(i);
}

HMM::HMM(std::string label, int numstates, vector<vector<double>> transp, std::vector<HMM_State*> states)
{
	_label = label;
	_numstates = numstates;
	_transp = transp;
	_states.assign (states.begin(),states.end());
}

void HMM::AddState(HMM_State* s)
{
	_states.push_back(s);
	_numstates = _states.size();
}

void HMM::AddHMMSerial(HMM* hmm)
{
	// build new transition probability matrix A
	vector<vector<double>> A;
	unsigned int newSize = 0;


	if (this->_numstates == 0) {
		this->_transp = hmm->_transp;
	}
	else {
		// transp1: n x n, transp2: m x m
		// new size: n+m-2 x n+m-2		
		newSize = this->_transp.size() + hmm->_transp.size() - 2;

		A.resize(newSize);

		// init with log(zero) (-infinity)
		for (unsigned int i=0; i<newSize; i++)
		{

			vector<double> a(newSize,log(0.0));
			A[i] = a;
		}

		// copy old matrix
		for (unsigned int j=0; j<_transp.size(); j++) {
			for (unsigned int i=0; i<_transp.size(); i++) {
				A[j][i] = _transp[j][i];
				//cout << "[" << j << "][" << i << _transp[j][i] << " ";
			}
			//cout << endl;
		}

		// copy new values
		unsigned int startpos = _transp.size()-1;
		for (unsigned int j=startpos; j<A.size(); j++) {
			for (unsigned int i=startpos; i<A.size(); i++) {
				A[j][i] = hmm->_transp[j-startpos+1][i-startpos+1]; // +1 because the initial state is skipped
				//cout << "[" << j << "][" << i << "] " << hmm->_transp[j][i] << " ";
			}
			//cout << endl;
		}

		this->_transp = A;
	}


	// add states
	for (unsigned int i=0; i<hmm->GetStates().size(); i++)
	{
		AddState(hmm->GetStates().at(i));
	}

}

void HMM::SetTransitionProbabilities(vector<vector<double>> p)
{
	_transp = p;
}


istream& operator >>(istream &is,HMM &obj)
{
	string line;
	vector<string> tokens;

	// name
	getline (is,line);
	tokens = HMM_Helper::Split(line);

	if (tokens.size() == 0)
		return is;

	// trim ""
	obj._label = tokens[1].substr(1, tokens[1].length()-2);

	cout << obj._label << " ";
	//beginhmm
	getline (is,line);
	// NUMSTATES
	getline (is,line);

	tokens = HMM_Helper::Split(line);
	obj._numstates = atoi(tokens[1].c_str());

	// read states
	for (int i=0; i<obj._numstates-2 && is.good() ; i++)
	{
		// state 
		getline(is, line);
		HMM_State* state = new HMM_State();

		is >> *state;

		obj._states.push_back(state);
	}
	
	// transp
	getline(is, line);
	int transpCnt = atoi(tokens[1].c_str());

	obj._transp.resize(transpCnt); // = new double*[transpCnt];

	for (int i=0; i<transpCnt; i++)
	{
		getline(is, line);
		obj._transp[i] = HMM_Helper::ConvertToDoubleVectorLog(line);
	}

	// endhmm
	getline (is,line);

	cout << endl;

	return is;
}